#!/usr/bin/env python
#
# Copyright (c) 2019 Intel Corporation
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
"""
receive nav_msgs::Odometry and publish geometry_msgs::PoseStamped
"""
import rospy
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry

pub = rospy.Publisher('gnss_pose', PoseStamped, queue_size=1)


def callback(data):
    """
    callback for current pose
    """
    pose = PoseStamped()
    pose.header = data.header
    pose.pose = data.pose.pose
    pub.publish(pose)


def convert_odometry_to_pose():
    """
    main loop
    """
    rospy.init_node('convert_odometry_to_pose', anonymous=True)
    role_name = rospy.get_param('/role_name', 'ego_vehicle')
    rospy.Subscriber('/carla/{}/odometry'.format(role_name), Odometry, callback)
    rospy.spin()


if __name__ == '__main__':
    convert_odometry_to_pose()
